#include "cameramicroepsilonplugin.h"

///
/// \brief controllostcallback
/// \param tmicro_epsilon
/// 这是scancontrol需要的回调函数，但是什么也没做
void controllostcallback( void * tmicro_epsilon )
{
    Q_UNUSED(tmicro_epsilon);
}

///
/// \brief CameraMicroEpsilonPlugin::CameraMicroEpsilonPlugin
/// \param parent
///
CameraMicroEpsilonPlugin::CameraMicroEpsilonPlugin(QObject *parent) :
    QGenericPlugin(parent)
{
    Q_UNUSED(parent);
}

///
/// \brief CameraMicroEpsilonPlugin::create
/// \param name
/// \param spec
/// \return
///
QObject* CameraMicroEpsilonPlugin::create(const QString& name, const QString &spec)
{
    Q_UNUSED(name);
    Q_UNUSED(spec);

    return nullptr;
}

///
/// \brief CameraMicroEpsilonPlugin::init
/// \param ip：ip地址
/// \param port：端口号
/// \return
///
bool CameraMicroEpsilonPlugin::init( string ip , uint16_t port )
{
    //保存通信参数，scancontrol实际上用不上
    Q_UNUSED(ip);
    Q_UNUSED(port);
    //scancontrol初始化
    int ret = 0;
    char *interfaces[MAX_INTERFACE_COUNT];
    guint32 resolutions[MAX_RESOLUTION];
    guint32 interface_count = 0;
    //目前空闲时间是614*10/1000 ms
    idle_time = 614;
    //目前是曝光时间1ms
    guint32 exposure_time = 100;
    //计算等待时间
    wait_time = (idle_time+exposure_time)*10;
    //查询有几个传感器连接到本机
    ret = CInterfaceLLT::GetDeviceInterfaces(&interfaces[0], MAX_INTERFACE_COUNT);
    if (ret == ERROR_GETDEVINTERFACE_REQUEST_COUNT)
    {
        std::cout << "目前已连接" << MAX_INTERFACE_COUNT << " 个激光轮廓仪" << std::endl;
        interface_count = MAX_INTERFACE_COUNT;
    }
    else if (ret < 1)
    {
        std::cout << "没有传感器连接到本机" << std::endl;
        this->error_str = R"(没有传感器连接到本机)";
        interface_count = 0;
    }
    else
    {
        interface_count = ret;
        if (interface_count == 0)
        {
            std::cout << "There is no scanCONTROL connected - Exiting" << std::endl;
            this->error_str = R"(没有scanCONTROL传感器连接本机)";
            goto cleanup;
        }
        else
        {
            std::cout << "There are " << interface_count << " scanCONTROL connected" << std::endl;
        }
        //实例话传感器控制接口
        hLLT = new CInterfaceLLT();
        //设置连接传感器的ID
        if ((hLLT->SetDeviceInterface(interfaces[0])) < GENERAL_FUNCTION_OK)
        {
            std::cout << "设置传感器控制参数出错" << std::endl;
            this->error_str = R"(设置传感器控制参数出错)";
            goto cleanup;
        }

        //连接传感器
        if ((ret = hLLT->Connect()) < GENERAL_FUNCTION_OK)
        {
            std::cout << "连接传感器出错，错误码 " << ret << "!" << std::endl;
            this->error_str = R"(连接传感器出错，错误码:)" + QString::asprintf("%d",ret).toStdString() ;
            goto cleanup;
        }
        //
        if ((ret = hLLT->GetLLTType(&llt_type)) < GENERAL_FUNCTION_OK) {
            std::cout << "获取传感器类型时发生错误!" << std::endl;
            this->error_str = R"(获取传感器类型时发生错误!)";
            goto cleanup;
        }
        //获取单条激光线轮廓数
        if ((ret = hLLT->GetResolutions(&resolutions[0], MAX_RESOLUTION)) < GENERAL_FUNCTION_OK) {
            std::cout << "获取分辨率时发生错误!" << std::endl;
            this->error_str = R"(获取分辨率时发生错误!)";
            goto cleanup;
        }
        resolution = resolutions[0];
        //设置为激光轮廓模式
        if (hLLT->SetProfileConfig(PROFILE) < GENERAL_FUNCTION_OK) {
            std::cout << "Error while setting SetProfileConfig!" << std::endl;
            this->error_str = R"(设置传感器为轮廓模式时发生错误!)";
            goto cleanup;
        }
        //初始化采样间隔
        if (hLLT->SetFeature(FEATURE_FUNCTION_IDLE_TIME, idle_time) < GENERAL_FUNCTION_OK) {
            std::cout << "Error while setting FEATURE_FUNCTION_IDLE_TIME!" << std::endl;
            this->error_str = R"(设置传感器空闲时间时发生错误!)";
            goto cleanup;
        }
        //设置曝光时间
        if (hLLT->SetFeature(FEATURE_FUNCTION_EXPOSURE_TIME, exposure_time) < GENERAL_FUNCTION_OK) {
            std::cout << "Error while setting FEATURE_FUNCTION_EXPOSURE_TIME!" << std::endl;
            this->error_str = R"(配置传感器曝光时间时发生错误!)";
            goto cleanup;
        }
        //设置为内触发模式
        if (hLLT->SetFeature(FEATURE_FUNCTION_TRIGGER, TRIG_INTERNAL) < GENERAL_FUNCTION_OK) {
            std::cout << "Error while setting FEATURE_FUNCTION_TRIGGER!" << std::endl;
            this->error_str = R"(设置传感器触发模式时发生错误!)";
            goto cleanup;
        }
        //注册传感器掉线回调函数
        if (hLLT->RegisterControlLostCallback((gpointer)controllostcallback, this) < GENERAL_FUNCTION_OK) {
            std::cout << "Error while registering control lost callback!" << std::endl;
            this->error_str = R"(注册传感器回调函数时发生错误!)";
            goto cleanup;
        }
        //启动轮廓传输，启动传输并一直获取轮廓
        if ((ret = hLLT->TransferProfiles(NORMAL_TRANSFER, true)) < GENERAL_FUNCTION_OK) {
            std::cout << "Error in profile transfer! " << ret << "" << std::endl;
            this->error_str = R"(启动轮廓传输发生错误!错误代码 = )" + QString::asprintf("%d",ret).toStdString();
            goto cleanup;
        }
        //启动传输后必须等待一段时间
        usleep(200 * 1000);
        goto func_end;
    }
cleanup:
    if( hLLT )
        delete hLLT;
    return false;
func_end:
    ret = 0;
    return true;
}

///
/// \brief CameraMicroEpsilonPlugin::dinit
/// \return
/// 反初始化
bool CameraMicroEpsilonPlugin::dinit( void )
{
    if( this->hLLT )
    {
        //停止传输
        hLLT->TransferProfiles(NORMAL_TRANSFER,false);
        //关闭接口
        delete  hLLT;
    }
    return true;
}

//获取通信参数
bool CameraMicroEpsilonPlugin::get_com_para( string &ip , uint16_t &port )
{
    ip = this->ip;
    port = this->port;
    return true;
}
//设置相机曝光
///
/// \brief CameraMicroEpsilonPlugin::set_exposure
/// \param exposure：曝光时间
/// \return
///
bool CameraMicroEpsilonPlugin::set_exposure( uint32_t exposure_time )
{
    //判定参数设定范围
    uint32_t uCameraShutter;
    if( exposure_time <= 2000)
    {
        uCameraShutter = exposure_time;
    }
    else
    {
        return false;
    }
    //避免重复设定
    if(u_exposure_time == uCameraShutter)
    {
        return true;
    }
    else
    {
        int nErr = 0;
        for(int i = 0; i < 5; i++) {
            nErr  = hLLT->SetFeature(FEATURE_FUNCTION_EXPOSURE_TIME, uCameraShutter);
            usleep(50*1000);
            if(nErr > 0)
            {
                u_exposure_time = uCameraShutter;
                //更新等待时间，等待时间不够144hz，那么必须等待如此长的时间，否则会死机
                if( idle_time+exposure_time < MINIMUM_WAITE_TIME )
                {
                    wait_time = 714;
                }//小于144hz按照设置时间等待
                else
                {
                    wait_time = (idle_time+exposure_time)*10;
                }
                return true;
            }
        }
        return false;
    }

}
///
/// \brief CameraMicroEpsilonPlugin::set_laser_mode
/// \param laser_mode：激光模式
/// \param laser_power：激光功率 scanControl没有激光功率控制接口
/// \return
///设置激光器模式
/// 如果是LASER_DIGTAL_POWER模式，可以通过laser_power直接设置激光器功率
bool CameraMicroEpsilonPlugin::set_laser_mode( _LASER_MODE laser_mode, uint32_t laser_power )
{
    Q_UNUSED(laser_power);

    int nErr = 0;
    guint32 LASER_MODE;
    switch (laser_mode)
    {
    case 0:
        LASER_MODE = LASER_FULL_POWER;
        break;
    case 1:
        LASER_MODE = LASER_REDUCED_POWER;
        break;
    case 2:
        LASER_MODE = LASER_FULL_POWER ^ LASER_PULSE_MODE;
        break;
    case 3:
        LASER_MODE = LASER_REDUCED_POWER ^ LASER_PULSE_MODE;
        break;
    case 4:
        LASER_MODE = LASER_OFF;
        break;
    default:
        LASER_MODE = LASER_FULL_POWER;
        break;
    }
    //避免重复设定
    if(m_nlasermode == laser_mode)
    {
        return m_nlasermode;
    }
    else
    {
        //重复5次 设定激光模式
        for(int i = 0; i < 5; i++)
        {
            nErr  = hLLT->SetFeature(FEATURE_FUNCTION_LASER, LASER_MODE);
            if(nErr > 0)
            {
                //返回激光状态，备用
                m_nlasermode = laser_mode;
                return true;
            }
            usleep(50*1000);
        }
    }
    return false;
}
//获取激光器轮廓
bool CameraMicroEpsilonPlugin::get_profiler( vector<double> &x,vector<double> &z )
{
    gint32 ret = 0;
    guint32 polled_data_size = 0;
    //如果初始化不成功直接返回
    if( hLLT == nullptr )
    {
        this->error_str = R"(传感器控制句柄未初始化)";
        return false;
    }
    profile_buffer.resize(resolution * 64);

    x.resize(resolution);
    z.resize(resolution);
    //一定要wait否则会获取不到正确的轮廓
    usleep(wait_time);
    //获取轮廓
    if ((polled_data_size = hLLT->GetActualProfile(&profile_buffer[0], profile_buffer.size(), PROFILE, NULL)) !=
        profile_buffer.size())
    {
        std::cout << "Error in profile transfer! " << ret << std::endl;
        this->error_str = R"(传输轮廓时发生错误)";
        return false;
    }
    //检查数据大小是否正确
    if (polled_data_size != profile_buffer.size())
    {
        std::cout << "Wrong profile size or polling error: " << polled_data_size << std::endl;
        this->error_str = R"(错误的激光轮廓大小)";
        return false;
    }
    //数据转换到输出容器中
    if ((ret = CInterfaceLLT::ConvertProfile2Values(&profile_buffer[0], profile_buffer.size(), resolution, PROFILE, llt_type, 0,
                                                    NULL, NULL, NULL, &x[0], &z[0], NULL, NULL)) !=
        (CONVERT_X | CONVERT_Z))
    {
        std::cout << "Error while extracting profiles" << std::endl;
        this->error_str = R"(解压激光轮廓数据时发生错误)";
        return false;
    }

    return true;
}

///
/// \brief CameraMicroEpsilonPlugin::get_error
/// \return
/// 获取错误原因
string CameraMicroEpsilonPlugin::get_error( void )
{
    return this->error_str;
}

///
/// \brief CameraMicroEpsilonPlugin::clear_error
/// \return
/// 清除错误
bool CameraMicroEpsilonPlugin::clear_error( void )
{
    this->error_str.clear();
    return true;
}


///以下都是未实现的接口
//x_s轮廓x提取起点，x_end轮廓x提取终点
bool CameraMicroEpsilonPlugin::set_camera_x_resolution( uint32_t x_start , uint32_t x_end )
{
    Q_UNUSED(x_start);
    Q_UNUSED(x_end);

    return true;
}
//设置传感器z方向视野起点和终点
bool CameraMicroEpsilonPlugin::set_camera_z_resolution( uint32_t z_start , uint32_t z_end )
{
    Q_UNUSED(z_start);
    Q_UNUSED(z_end);

    return true;
}
//获取传感器x轴分辨率
bool CameraMicroEpsilonPlugin::get_camera_x_resolution( uint32_t &x_start , uint32_t &x_end )
{
    Q_UNUSED(x_start);
    Q_UNUSED(x_end);

    return true;
}
//获取传感器z方向视野起点和终点
bool CameraMicroEpsilonPlugin::get_camera_z_resolution( uint32_t &z_start , uint32_t &z_end )
{
    Q_UNUSED(z_start);
    Q_UNUSED(z_end);

    return true;
}

//设置传感器增益参数
//scanControl未实现
bool CameraMicroEpsilonPlugin::set_camera_gain( uint32_t gain )
{
    Q_UNUSED(gain);

    return true;
}

#if QT_VERSION < 0x050000
Q_EXPORT_PLUGIN2(CameraMicroEpsilonPlugin, CameraMicroEpsilonPlugin)
#endif // QT_VERSION < 0x050000
